#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include "core/common/core.h"
#include "perception/costmap2d/costmap_2d.h"

class TestCostmap{
    public:
        TestCostmap(tf2_ros::Buffer* tf_buffer);
        ~TestCostmap();

        // 代价地图发布
        void CostmapLoopPub();

    private:
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // 二维雷达回调函数
        void LidarCallback(const sensor_msgs::LaserScan::Ptr& msg);
        // 融合数据回调函数
        void FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg);
        // 里程计回调函数
        void OdometryCallback(const nav_msgs::Odometry::Ptr& msg);
        void MatchedPointsCallback(const sensor_msgs::PointCloud2::Ptr& msg);
        // 深度回调函数
        void FusionDepthCallback(const sensor_msgs::PointCloud2::Ptr& msg);
    private:
        ros::NodeHandle nh_;
        tf2_ros::Buffer* const tfBuffer_;
        ros::Subscriber mapSub_; 
        ros::Subscriber lidarScanSub_;
        ros::Subscriber fusionDataSub_;
        ros::Subscriber matchedPointsSub_;
        ros::Subscriber fusionDepthSub_;
        ros::Subscriber odomSub_;
        ros::Publisher costmapLocalPub_;
        ros::Publisher costmapGlobalPub_;

        boost::shared_ptr<const nav_msgs::Odometry> odomPtr_;

        boost::shared_ptr<sensor_msgs::PointCloud2> fusionDataPtr_;         //传感器融合数据
        boost::shared_ptr<costmap_2d::Costmap2D> costmapLocalPtr_;
        boost::shared_ptr<costmap_2d::Costmap2D> costmapGlobalPtr_;
};


TestCostmap::TestCostmap(tf2_ros::Buffer* tf_buffer):tfBuffer_(tf_buffer),odomPtr_(nullptr){
    costmapGlobalPtr_ = boost::make_shared<costmap_2d::Costmap2D>("global_costmap", false, tfBuffer_);
    costmapLocalPtr_ = boost::make_shared<costmap_2d::Costmap2D>("local_costmap", true, tfBuffer_);

    costmapGlobalPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/global_costmap", 1);
    costmapLocalPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/local_costmap", 1);
    mapSub_ = nh_.subscribe("/map", 10, &TestCostmap::MapCallback, this);
    // lidarScanSub_ = nh_.subscribe("/scan_raw", 10, &TestCostmap::LidarCallback, this);
    // fusionDataSub_ = nh_.subscribe("/fusion_bev", 10, &TestCostmap::FusionDataCallback, this);
    odomSub_ = nh_.subscribe("/odom", 10, &TestCostmap::OdometryCallback, this);
    matchedPointsSub_ = nh_.subscribe("/scan_matched_points2", 10, &TestCostmap::MatchedPointsCallback, this);
    // fusionDepthSub_ = nh_.subscribe("/fusion_depth", 10, &TestCostmap::FusionDepthCallback, this);
    CostmapLoopPub();
}

TestCostmap::~TestCostmap(){

}

// 地图回调函数
void TestCostmap::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    LOG(INFO) << "[MapCallback] Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution;
    
    costmapGlobalPtr_->setMapData(msg);
    costmapLocalPtr_->setMapData(msg);
}

// 二维雷达回调函数
void TestCostmap::LidarCallback(const sensor_msgs::LaserScan::Ptr& msg){
    costmapGlobalPtr_->setLidar2DScanData(msg);
    costmapLocalPtr_->setLidar2DScanData(msg);
}

// 里程计回调函数
void TestCostmap::OdometryCallback(const nav_msgs::Odometry::Ptr& msg){
    odomPtr_ = msg;
}


// 融合数据回调函数
void TestCostmap::FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg){
    fusionDataPtr_ = msg;
    costmapGlobalPtr_->setPointCloud2Data(fusionDataPtr_);
    costmapLocalPtr_->setPointCloud2Data(fusionDataPtr_);
}

void TestCostmap::MatchedPointsCallback(const sensor_msgs::PointCloud2::Ptr& msg){
    costmapGlobalPtr_->setPointCloud2Data(msg);
    costmapLocalPtr_->setPointCloud2Data(msg);
}

void TestCostmap::FusionDepthCallback(const sensor_msgs::PointCloud2::Ptr& msg){
    costmapGlobalPtr_->setPointCloud2Data(msg);
    costmapLocalPtr_->setPointCloud2Data(msg);
}


// 代价地图发布
void TestCostmap::CostmapLoopPub(){
    ros::Rate loop_rate = ros::Rate(50);
    nav_msgs::OccupancyGrid local_map;
    nav_msgs::OccupancyGrid global_map;

    while(!ros::isShuttingDown()){
        loop_rate.sleep();
        ros::spinOnce();
        ros::Time now = ros::Time::now();
        // double time_interval = (now - global_map.header.stamp).toSec();
        // cout << "time_interval: " << time_interval << endl;

        // if(odomPtr_ && fabs(odomPtr_->twist.twist.angular.z) > 0.2){
        //     ROS_WARN_STREAM("odomPtr_->twist.twist.angular.z: " << odomPtr_->twist.twist.angular.z);
        //     continue;
        // }

        if(costmapGlobalPtr_){
            if((now - global_map.header.stamp).toSec() > 3.0){
                ros::Time start = ros::Time::now();
                costmapGlobalPtr_->updateCostmap();
                costmapGlobalPtr_->getOccupancyGrid(global_map);
                costmapGlobalPub_.publish(global_map);
                double time_interval = (ros::Time::now() - start).toSec();
                ROS_INFO_STREAM("Publish global_map......, time_interval: " << time_interval);
            }
        }

        if(costmapLocalPtr_){
            if((now - local_map.header.stamp).toSec() > 0.1){
                ros::Time start = ros::Time::now();
                // local_map.header.stamp = start;
                costmapLocalPtr_->updateCostmap();
                costmapLocalPtr_->getOccupancyGrid(local_map);
                costmapLocalPub_.publish(local_map);
                double time_interval = (ros::Time::now() - start).toSec();
                ROS_INFO_STREAM("Publish local_costmap......, time_interval: " << time_interval);
            }
        }
    }
}


int main(int argc, char** argv){
    ros::init(argc, argv, "test_costmap");

    string glog_dir = ros::param::param<string>("/glog_dir", "/home/xxty/user_config/logs");
    common::glog::init(glog_dir, "test_costmap");
    
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("test_costmap start...");

    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    TestCostmap test_costmap(&tfBuffer);
    ros::spin();

    common::glog::shutdown();

    return 0;
}